逆向運動學函式 << Previous Next >> W17

Python remote API 逆向運動學函式

影片:

主程式:

-- Put some initialization code here:
sim.setThreadSwitchTiming(2) -- Default timing for automatic thread switching

-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
    sim.addStatusbarMessage('Lua runtime error: '..err)
end

-- Put some clean-up code here:
simRemoteApi.start(19997)

吸盤程式:

function sysCall_init() 
    --this is teach by 40823214
    objectHandle=sim.getObjectHandle('suctionPad')
    sim.setUserParameter(objectHandle,'@enable','')
    modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)
    robotBase=modelBase
    while true do
        robotBase=sim.getObjectParent(robotBase)
        if robotBase==-1 then
            robotName='Dobot'
            break
        end
        robotName=sim.getObjectName(robotBase)
        suffix,suffixlessName=sim.getNameSuffix(robotName)
        if suffixlessName=='Dobot' then
            break
        end
    end
 
    s=sim.getObjectHandle('suctionPadSensor')
    l=sim.getObjectHandle('suctionPadLoopClosureDummy1')
    l2=sim.getObjectHandle('suctionPadLoopClosureDummy2')
    b=sim.getObjectHandle('suctionPad')
    suctionPadLink=sim.getObjectHandle('suctionPadLink')
    local gripperBase=sim.getObjectAssociatedWithScript(sim.handle_self)
 
    infiniteStrength=sim.getScriptSimulationParameter(sim.handle_self,'infiniteStrength')
    maxPullForce=sim.getScriptSimulationParameter(sim.handle_self,'maxPullForce')
    maxShearForce=sim.getScriptSimulationParameter(sim.handle_self,'maxShearForce')
    maxPeelTorque=sim.getScriptSimulationParameter(sim.handle_self,'maxPeelTorque')
 
    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
end
 
function sysCall_cleanup() 
    --this is teach by 40823214
    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
end
 
function sysCall_sensing() 
    parent=sim.getObjectParent(l)
    --this is teach by 40823214
    local sig=sim.getIntegerSignal("pad_switch")
    if (not sig) or (sig==0) then
        if (parent~=b) then
            sim.setLinkDummy(l,-1)
            sim.setObjectParent(l,b,true)
            m=sim.getObjectMatrix(l2,-1)
            sim.setObjectMatrix(l,-1,m)
        end
    else
        if (parent==b) then
            index=0
            while true do
                shape=sim.getObjects(index,sim.object_shape_type)
                if (shape==-1) then
                    break
                end
                local res,val=sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)
                if (shape~=b) and (val~=0) and (sim.checkProximitySensor(s,shape)==1) then
                    -- Ok, we found a respondable shape that was detected
                    -- We connect to that shape:
                    -- Make sure the two dummies are initially coincident:
                    sim.setObjectParent(l,b,true)
                    m=sim.getObjectMatrix(l2,-1)
                    sim.setObjectMatrix(l,-1,m)
                    -- Do the connection:
                    sim.setObjectParent(l,shape,true)
                    sim.setLinkDummy(l,l2)
                    break
                end
                index=index+1
            end
        else
            -- Here we have an object attached
            if (infiniteStrength==false) then
                -- We might have to conditionally beak it apart!
                result,force,torque=sim.readForceSensor(suctionPadLink) -- Here we read the median value out of 5 values (check the force sensor prop. dialog)
                if (result>0) then
                    breakIt=false
                    if (force[3]>maxPullForce) then breakIt=true end
                    sf=math.sqrt(force[1]*force[1]+force[2]*force[2])
                    if (sf>maxShearForce) then breakIt=true end
                    if (torque[1]>maxPeelTorque) then breakIt=true end
                    if (torque[2]>maxPeelTorque) then breakIt=true end
                    if (breakIt) then
                        -- We break the link:
                        sim.setLinkDummy(l,-1)
                        sim.setObjectParent(l,b,true)
                        m=sim.getObjectMatrix(l2,-1)
                        sim.setObjectMatrix(l,-1,m)
                    end
                end
            end
        end
    end
end

MTB_robot remote API程式:

import sim as vrep
import math
import random
import time
import keyboard
  
  
print ('Start')
  
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
  
if clientID != -1:
    print ('Connected to remote API server')
      
    res = vrep.simxAddStatusbarMessage(
        clientID, "40823129",
        vrep.simx_opmode_oneshot)
    if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
        print("Could not add a message to the status bar.")
  
      
    opmode = vrep.simx_opmode_oneshot_wait
    angle1=math.pi/180
      
    # radian to degree
    deg = 180/math.pi
    # link 1 length
    a1 = 0.468
    # link 2 length
    a2 = 0.4
    # derivated based up https://www.youtube.com/watch?v=IKOGwoJ2HLk&t=311s
       
    def ik(x, y):
    # (x, y)  need to be located inside the circle with radius a1+a2
       if (x**2 + y**2) <= (a1+ a2)**2:
          q2 = math.acos((x**2+y**2-a1**2-a2**2)/(2*a1*a2))
          q1 = math.atan2(y, x) - math.atan2((a2*math.sin(q2)), (a1+a2*math.cos(q2)))
        # The decimal point of number is rounded to the 4th place
          return [round(q1*deg, 4), round(q2*deg, 4)]
       else:
         print("Over range!")
        # end the script execution
 
  
    theta = ik(0.2, 0.7)
  
    print(theta[0], theta[1])
 
 
    ret,axis2=vrep.simxGetObjectHandle(clientID,"MTB_axis2",opmode)
    ret,axis1=vrep.simxGetObjectHandle(clientID,"MTB_axis1",opmode)
    ret,axis4=vrep.simxGetObjectHandle(clientID,"MTB_axis4",opmode)
    ret,suctionPad=vrep.simxGetObjectHandle(clientID,"suctionPad",opmode)
    vrep.simxSetJointTargetPosition(clientID,axis1,theta[0]*angle1,opmode)
    vrep.simxSetJointTargetPosition(clientID,axis2,theta[1]*angle1,opmode)
    vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
    time.sleep(0.5)
    while True:
            vrep.simxSetJointPosition(clientID,axis4,-0.15,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,0.01,opmode)
            theta = ik(0.2, 0.7)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,0.01,opmode)
            vrep.simxSetJointPosition(clientID,axis2,theta[0]*angle1,opmode)
            vrep.simxSetJointPosition(clientID,axis1,theta[1]*angle1,opmode)
            time.sleep(2)
            vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,-0.03,opmode)
            time.sleep(0.5)
            vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,0.03,opmode)
            time.sleep(0.5)
            theta = ik(-0.3, -0.55)
            vrep.simxSetJointPosition(clientID,axis4,0.01,opmode)
            vrep.simxSetJointPosition(clientID,axis2,theta[0]*angle1,opmode)
            vrep.simxSetJointPosition(clientID,axis1,theta[1]*angle1,opmode)
            time.sleep(2)
            vrep.simxSetIntegerSignal(clientID,"pad_switch",0,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,0.01,opmode)
            time.sleep(0.5)
            vrep.simxSetJointPosition(clientID,axis4,-0.03,opmode)
            time.sleep(0.5)
            vrep.simxSetIntegerSignal(clientID,"pad_switch",1,opmode)
    end
          
else:
    print ('Failed connecting to remote API server')
    print ('End')

Tips:

需添加以下3個檔案並放在同一個資料夾

逆向運動學函式 << Previous Next >> W17